#include <opencv2/opencv.hpp>
#include <iostream>
#include <RMVideoCapture.h>
using namespace cv;
using namespace std;
int iLowH = 83;int iHighH = 107;
int jLowH = 83;int jHighH = 107;
int iLowS = 0;int iHighS = 255;
int iLowV = 230;int iHighV = 255;
int main(int argc,char** argv)
{
    RMVideoCapture cap("/dev/video1", 3);
    cap.setVideoFormat(640, 360, 1);
    int dx = 0;
    int ddx = 0;
    char ad[128] = {0};
    cv::namedWindow("Control");
    cvCreateTrackbar("LowH", "Control", &iLowH, 255); //Hue (0 - 179)
    cvCreateTrackbar("HighH", "Control", &iHighH, 255);
    cvCreateTrackbar("jLowH", "Control", &jLowH, 255); //Hue (0 - 179)
    cvCreateTrackbar("jHighH", "Control", &jHighH, 255);
    cvCreateTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
    cvCreateTrackbar("HighS", "Control", &iHighS, 255);
    cvCreateTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255)
    cvCreateTrackbar("HighV", "Control", &iHighV, 255);
    //115 is hight
    //20 is low
    int exp_t = 20000;
    cap.setExposureTime(0, exp_t);//settings->exposure_time);
    cap.startStream();
    cap.info();
    while(1){
        Mat src;
        cap >> src;

        Mat imgHSV, mask;
        cvtColor(src, imgHSV, COLOR_BGR2HSV);
//        inRange(imgHSV, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), mask1);
        if(0){
            //blue
            //inRange(imgHSV, Scalar(61, 0 ,218), Scalar(126, 182, 255), mask); //Threshold the image
            //inRange(imgHSV,Scalar(60,0,235), Scalar(140, 255, 255),mask);//680x480
            inRange(imgHSV,Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV),mask);//680x480

            //inRange(imgHSV,Scalar(107,0,39), Scalar(135, 255, 255),mask);
        }
        else{
            //red
            Mat1b mask1, mask2;
            inRange(imgHSV, Scalar(iLowH,iLowS,iLowV), Scalar(iHighH,iHighS,iHighV), mask1);
            inRange(imgHSV, Scalar(jLowH,iLowS,iLowV), Scalar(jHighH,iHighS,iHighV), mask2);
            mask = mask1| mask2;
        }
//        imshow("hsv", mask);

        char key = waitKey(1);
        bool save_flag = false;
        switch(key)
        {
        case 'w':
            exp_t += 1;
            cap.setExposureTime(0, exp_t);//settings->exposure_time);
            cout << "current exp t:\t" << exp_t << endl;
            break;
        case 's':
            exp_t -= 1;
            cap.setExposureTime(0, exp_t);
            cout << "current exp t:\t" << exp_t << endl;
            break;

        case 'm':
            save_flag = true;
            break;

        case 'a':
            sprintf(ad,"/home/cz/Calibration/buff_8mm_0/chessboard%02d.jpg", ++dx);
            //imshow(ad, src);
            imwrite(ad,src);
            cout<<"saved"<<endl;
            break;
        case 'q':
            return -1;
        }
        if(save_flag)
        {
            sprintf(ad,"/home/cz/zhenlvtest/chessboard%02d.jpg", ++ddx);
            //imshow(ad, src);
            imwrite(ad,src);
        }
        putText(src,to_string(dx),Point(320,180),FONT_HERSHEY_COMPLEX,4,Scalar(0,0,255),4);
        namedWindow("src", 0);
        imshow("src", src);
    }
}

